from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import QShortcut
from PyQt5.QtGui import QKeySequence
from PyQt5.QtCore import pyqtSignal, QObject
import ui.main_window as main_window
import sys
import json
import threading
import queue
import time
from consts import *

class SignalEmitter(QObject):
   msg_browser_emitter = pyqtSignal(str)
   msg_num_led_emitter = pyqtSignal(int, str)
   statusbar_emitter   = pyqtSignal(str)
   port_change_emitter = pyqtSignal(int, str)
   def __init__(self):
       super().__init__()

def rount_cut(raw_angle):
    if raw_angle > 360:
        angle = raw_angle % 360
    elif raw_angle < 0:
        angle = 360 - raw_angle
    else: 
        angle = raw_angle
    return angle

if __name__ == "__main__":
    import joystick 
    import robot_control
    import udp_client
    import serial_client

    APP = QtWidgets.QApplication(sys.argv)
    MAINWINDOW = QtWidgets.QMainWindow()
    UI = main_window.Ui_MainWindow()
    UI.setupUi(MAINWINDOW)

    JOYSTICK_MANAGER = joystick.Joystick_Manager()
    ROBOT = robot_control.Robot()
    DEBUG_COMMUICATOR = robot_control.Base_Communicator()
    SERIAL_MANAGER = serial_client.Serial_Manager()
    threading.Thread(target=ROBOT.main_loop, args=(JOYSTICK_MANAGER.joystick_outputs,)).start()
    GUI_SIGANLS_QUEUE = queue.Queue()
    ROBOT.set_gui_signal_que(GUI_SIGANLS_QUEUE)
    SERIAL_MANAGER.set_gui_signal_que(GUI_SIGANLS_QUEUE)

    CURSOR = UI.textBrowser.textCursor()
    UI_SIGNALS = SignalEmitter()
    BUTTONS_STATE = [False for _ in range(6)]
else:
    APP = None
    MAINWINDOW = None
    UI = None
    JOYSTICK_MANAGER = None
    ROBOT = None
    SERIAL_MANAGER = None
    DEBUG_COMMUICATOR = None
# UDP_CLIENT = udp_client.Udp_Communicator()
# CONFIG = {}
# with open("config.json", "r") as f:
#         CONFIG:dict = json.load(f)

def signal_handler():
    """后端信息处理及展示"""
    last_time = [time.time() for _ in range(4)]
    pos = [0 for _ in range(3)]
    motor_table = [[0, ROBOT.config["axis_1"]["out_min"]], 
                   [0, -ROBOT.config["axis_1"]["out_min"]], 
                   [3, ROBOT.config["axis_3"]["out_max"]], 
                   [3, -ROBOT.config["axis_3"]["out_max"]],
                   [1, -ROBOT.config["axis_5"]["out_max"]], 
                   [1, ROBOT.config["axis_5"]["out_max"]], 
                   ]
    flags = [False for _ in range(3)]
    while GUI_SIGANLS_QUEUE is not None:
        if not GUI_SIGANLS_QUEUE.empty():
            args = GUI_SIGANLS_QUEUE.get().split(" ")
            if args[0] == f"{MOTOR_SPEED}":
                if args[1] == "0":
                    spd = float(args[2])
                    if abs(spd) > 0.1:
                        # 上升沿触发：清空计时
                        if not flags[0]:
                            last_time[0] = time.time()
                        flags[0] = True
                    else:
                        flags[0] = False
                    if flags:
                        dt = time.time() - last_time[0]
                        pos[0] += spd * dt
                        UI_SIGNALS.msg_num_led_emitter.emit(0, f"{spd:.1f}")
                        UI_SIGNALS.msg_num_led_emitter.emit(3, f"{pos[0]:.1f}")
                    last_time[0] = time.time()
                elif args[1] == "3":
                    dt = time.time() - last_time[1]
                    pos[1] += float(args[2]) * dt
                    UI_SIGNALS.msg_num_led_emitter.emit(1, f"{float(args[2]):.1f}")
                    UI_SIGNALS.msg_num_led_emitter.emit(4, f"{pos[1]:.1f}")
                    last_time[1] = time.time()
                elif args[1] == "1":
                    dt = time.time() - last_time[2]
                    pos[2] += float(args[2]) * dt
                    pos[2] = rount_cut(pos[2])
                    UI_SIGNALS.msg_num_led_emitter.emit(2, f"{float(args[2]):.1f}")
                    UI_SIGNALS.msg_num_led_emitter.emit(5, f"{pos[2]:.1f}")
                    last_time[2] = time.time()
            
            elif args[0] == f"{SERIAL_CANNOT_OPEN}":
                UI_SIGNALS.port_change_emitter.emit(0, "无法开启此端口")
                msg = ""
                for i in args[1:]:
                    msg += f"{i} "
                UI_SIGNALS.statusbar_emitter.emit(msg)
            elif args[0] == f"{SPD_WRITE_ERROR}":
                UI_SIGNALS.port_change_emitter.emit(0, "连接失败")
                msg = ""
                for i in args[1:]:
                    msg += f"{i} "
                UI_SIGNALS.statusbar_emitter.emit(msg)
            elif args[0] == f"{NEW_CONNECTION}":
                last_time = [time.time() for _ in range(4)]
                pos = [0 for _ in range(3)]
                msg = ""
                for i in args[1:]:
                    msg += f"{i} "
                UI_SIGNALS.statusbar_emitter.emit(f"new connection {msg}")
            
            elif args[0] == f"{GET_MSG}":
                msg = ""
                for i in args[1:]:
                    msg += f"{i} "
                msg = msg.replace("\\n", "\n")
                msg = msg.replace("\\t", "   ")
                msg = msg.replace("\\r", " ")
                msg = msg[2:-2]
                UI_SIGNALS.msg_browser_emitter.emit(msg)
        for k, state in enumerate(BUTTONS_STATE):
            if state:
                if time.time() - last_time[3] > 0.05:
                    ROBOT.set_speed(*motor_table[k])
                    last_time[3] = time.time()


def slot_show_msg(msg:str):
    CURSOR.insertText(msg)
    UI.textBrowser.setTextCursor(CURSOR)

def slot_num_led_show(index:int, spd:str):
    funcs = [
        lambda x: UI.cath_speed_lcd.display(x),
        lambda x: UI.wire_speed_lcd.display(x),
        lambda x: UI.wire_rotSpeed_lcd.display(x),
        lambda x: UI.cath_pos_speed_lcd.display(x),
        lambda x: UI.wire_pos_lcd.display(x),
        lambda x: UI.wire_rotPos_lcd.display(x),
    ]
    funcs[index](spd)

def slot_status_show(msg:str):
    UI.statusbar.showMessage(msg)

def slot_change_port(index:int, msg:str=None):
    UI.com_select.setCurrentIndex(index)
    if msg:
        UI.com_select.setItemText(index, msg)


def func_for_send_msg(*args):
    """发送指令"""
    msg = UI.msgInput.text()
    ROBOT.communicator.write(msg.encode())
    CURSOR.insertHtml(f"<span style='color: green;'>发送指令</span>: <span style='color: blue;'>{msg}</span><br>\n")
    UI.msgInput.clear()

def func_for_emergency_stop(*args):
    """急停开关执行的指令"""
    ROBOT.emergency_stop()
    UI.joystick_select.setCurrentIndex(0)

def func_for_show_joysticks(*args):
    last_joys = JOYSTICK_MANAGER.joy_list.copy()
    joys = JOYSTICK_MANAGER.scan_joystick()
    for k, i in enumerate(last_joys):
        if i not in joys:
            UI.joystick_select.removeItem(k+1)
    for i in joys:
        if i not in last_joys:
            UI.joystick_select.addItem(i)
    UI.joystick_select.showPopup()

def func_for_select_joystick():
    if UI.joystick_select.currentIndex() != 0:
        # JOYSTICK_MANAGER.joystick_outputs
        JOYSTICK_MANAGER.start_joystick(UI.joystick_select.currentIndex()-1)
        
    else:
        JOYSTICK_MANAGER.stop_joystick()

def func_for_show_ports(*args):
    """展示可用端口的函数"""
    UI.com_select.setItemText(0, "断开连接")
    UI.com_select.setItemText(1, f"UDP: {UI.ip_input_line.text()}:{udp_client.LOCAL_PORT}")
    last_ports = SERIAL_MANAGER.ports.copy()
    ports, names = SERIAL_MANAGER.scan_ports()
    for k, i in enumerate(last_ports):
        # 删除已不存在的端口
        if i not in ports:
            UI.com_select.removeItem(k+2)
            if last_ports:
                last_ports.pop(k)
    
    for k, i in enumerate(names):
        # 添加新出现的端口
        if ports[k] not in last_ports:
            UI.com_select.addItem(i)
            if last_ports:
                last_ports.append(ports[k])
    UI.com_select.showPopup()


def func_for_select_port(*args):
    """使用可用端口的函数"""
    index = args[0]
    ROBOT.set_communicator(DEBUG_COMMUICATOR)
    if index == 0:
        UI_SIGNALS.statusbar_emitter.emit("disconnect")
    elif index == 1:
        GUI_SIGANLS_QUEUE.put(f"{NEW_CONNECTION} {UI.ip_input_line.text()}")
        if udp_client.is_valid_ip(UI.ip_input_line.text()):
            ROBOT.set_communicator(udp_client.Udp_Communicator(UI.ip_input_line.text()))
        else:
            UI.com_select.setItemText(0, f"无效IP")
            UI.com_select.setCurrentIndex(0)
            UI.ip_input_line.setText(udp_client.CONFIG['RemoteIP'])
    else:
        GUI_SIGANLS_QUEUE.put(f"{NEW_CONNECTION} {UI.com_select.currentText()}")
        ROBOT.set_communicator(SERIAL_MANAGER.start_port(SERIAL_MANAGER.ports[index-2]))
        

def func_for_emrgency_stop(*args):
    ROBOT.emergency_stop()
    UI.joystick_select.setCurrentIndex(0)


def save_config(config):
    # 同步配置
    with open("config.json", 'w') as js_file:
        json.dump(config, js_file, indent=4)
    return config

def func_for_button_control_start(id):
    def innner(*args):
        global BUTTONS_STATE
        nonlocal id
        JOYSTICK_MANAGER.pause_joystick()
        BUTTONS_STATE[id] = True
    return innner


def func_for_button_control_stop(id):
    JOYSTICK_MANAGER.continue_joystick()
    ids = [0, 3, 1]
    def innner(*args):
        global BUTTONS_STATE
        nonlocal id
        JOYSTICK_MANAGER.continue_joystick()
        BUTTONS_STATE[id] = False
        time.sleep(0.01)
        ROBOT.set_speed(ids[id//2], 0.)
        # UI_SIGNALS.msg_num_led_emitter.emit(id//2, "0.00")
    return innner


def bind_methods():
    UI.gear_level_slider.setValue(5)
    UI.joystick_select.mousePressEvent = func_for_show_joysticks
    UI.joystick_select.currentIndexChanged.connect(func_for_select_joystick)
    UI.joystick_select.wheelEvent=lambda *args: None 

    # port_select
    UI.com_select.addItem(f"UDP: {udp_client.CONFIG['RemoteIP']}:{udp_client.LOCAL_PORT}")
    UI.com_select.mousePressEvent = func_for_show_ports
    UI.com_select.currentIndexChanged.connect(func_for_select_port) 
    UI.com_select.wheelEvent=lambda *args: None
    UI.ip_input_line.setText(udp_client.CONFIG['RemoteIP'])

    shortcut = QShortcut(QKeySequence(u'Return'), MAINWINDOW)
    shortcut2 = QShortcut(QKeySequence(u'Enter'), MAINWINDOW)
    shortcut.activated.connect(func_for_send_msg)
    shortcut2.activated.connect(func_for_send_msg)
    UI.Sendbutton.clicked.connect(func_for_send_msg)

    UI.all_stop_button.clicked.connect(func_for_emergency_stop)

    UI_SIGNALS.msg_browser_emitter.connect(slot_show_msg)
    UI_SIGNALS.msg_num_led_emitter.connect(slot_num_led_show)
    UI_SIGNALS.statusbar_emitter.connect(slot_status_show)
    UI_SIGNALS.port_change_emitter.connect(slot_change_port)

    # buttons
    UI.cath_push_button.mousePressEvent = func_for_button_control_start(0)
    UI.cath_pull_button.mousePressEvent = func_for_button_control_start(1)
    UI.wire_push_button.mousePressEvent = func_for_button_control_start(2)
    UI.wire_pull_button.mousePressEvent = func_for_button_control_start(3)
    UI.wire_cw_button.mousePressEvent = func_for_button_control_start(4)
    UI.wire_ccw_button.mousePressEvent = func_for_button_control_start(5)

    UI.cath_push_button.mouseReleaseEvent = func_for_button_control_stop(0)
    UI.cath_pull_button.mouseReleaseEvent = func_for_button_control_stop(1)
    UI.wire_push_button.mouseReleaseEvent = func_for_button_control_stop(2)
    UI.wire_pull_button.mouseReleaseEvent = func_for_button_control_stop(3)
    UI.wire_cw_button.mouseReleaseEvent = func_for_button_control_stop(4)
    UI.wire_ccw_button.mouseReleaseEvent = func_for_button_control_stop(5)

def at_exit(*args):
    global GUI_SIGANLS_QUEUE
    ROBOT._loop_running = False
    GUI_SIGANLS_QUEUE = None

def main():
    threading.Thread(target=signal_handler).start()
    bind_methods()
    MAINWINDOW.closeEvent = at_exit
    MAINWINDOW.show()
    sys.exit(APP.exec_())


if __name__ == "__main__":
    print("start GUI")
    main()
